#!/usr/bin/env python
#-*-coding:utf-8-*-

# 加载ros的Python基础包
import rospy
# 加载topic话题 的 msg消息
from std_msgs.msg import Float64
from geometry_msgs.msg import Twist,Pose
from nav_msgs.msg import Odometry
from gazebo_msgs.msg import LinkStates
from std_msgs.msg import Header
import numpy as np
import math
a=0

def servo_commands():
    # 创建node节点 —— 电机控制
    rospy.init_node('motor_control', anonymous=True)      
    # 订阅topic话题 —— 电机pwm输出
    rospy.Subscriber('/gazebo/link_states', LinkStates, sub_robot_pose_update)
    rospy.Subscriber("/teb_cmd_vel", Twist, callback)
    # 阻塞等待
    rospy.spin()
# 回调函数

def sub_robot_pose_update(msg):
        global a
        cmd = Twist()
        global  goal_posex
        global  goal_posey 
        goal_posex = -0.05
        goal_posey = -5.10
        # Find the index of the racecar 找到赛车的索引
        try:
            #创建msg消息对象
            arrayIndex = msg.name.index('robot::dummy')
        except ValueError as e:
            # Wait for Gazebo to startup 等待Gazebo启动
            pass
        else:
            # Extract our current position information 提取我们当前的位置信息
            last_received_pose = msg.pose[arrayIndex]
            last_received_twist = msg.twist[arrayIndex]
            if (abs(last_received_pose.position.x - goal_posex)<0.05) and (abs(last_received_pose.position.y - goal_posey)<0.05):
                a =1
            else:
                a=0

def callback(data):
    global a
    pub_cmd = rospy.Publisher('/cmd_vel', Twist, queue_size=10)  
    # 创建 msg 消息, 注意:ros的float64是一个结构体
    vel_msg = Twist()
    if a==1:
        vel_msg.linear.x = 0
        vel_msg.angular.z = 0
    else:
        vel_msg.linear.x = data.linear.x
        vel_msg.angular.z =data.angular.z
    pub_cmd.publish(vel_msg)
   # 发布topic话题 —— 线速度输出
    # 向topic话题 发送 msg消息

if __name__ == '__main__':
    try:
            servo_commands()
    except rospy.ROSInterruptException:
        pass

